/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCamera.*;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.*;
import edu.wpi.first.wpilibj.image.NIVision.*;
import java.util.Timer.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Frank extends IterativeRobot {
    Joystick     GPad, GPadDrive;
    
    Jaguar  sweeper;
    Victor  conveyer;
    
    CANJaguar  shooterA;
    CANJaguar shooterB;
    CANJaguar shooterC;
    
    //Victor  Aiming;
    
    Victor BridgeArm;
    
    RobotDrive drive;
    
    AxisCamera  Camera;                                                            
    ColorImage  Image;                                                            
    BinaryImage BI;
    
    CriteriaCollection Collection;
    
    Relay light;
    Relay Solonoid;
    
    Timer Timer;
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        try {
            GPad = new Joystick(1);
            GPadDrive = new Joystick(2);
   
            
            sweeper         = new Jaguar(5);
            conveyer        = new Victor(6);
            
            shooterA        = new CANJaguar(10);
            shooterB        = new CANJaguar(7);
            shooterC        = new CANJaguar(8);
            
            //Aiming          = new Victor(12);
            
            //BridgeArm       = new Victor(6); 
            
            drive = new RobotDrive(1, 2, 3, 4);
        
            drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, false);
            drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, false);
            drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false);
            drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, false);
            
            light    = new Relay(2);
            light.setDirection(Relay.Direction.kForward);
            Solonoid = new Relay(1);
            Solonoid.setDirection(Relay.Direction.kForward);
        
            Camera = AxisCamera.getInstance("10.26.58.11");                                        
            Camera.writeResolution(AxisCamera.ResolutionT.k640x480);                  
            Camera.writeBrightness(25);                                               
            Camera.writeCompression(0);                                               
            
            Timer = new Timer();
            
                        
            Collection = new CriteriaCollection();
            Collection.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 0, 50, false);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }



    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
 
        Solinoid();
        BallPickDown();
        Shooter();
        
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
         Drive();
        if(GPad.getRawButton(1)){
            BallPickUp();
        } 
        if(GPad.getRawButton(4)){
            BallPickDown();
        }else{
            BallPickStop();
        }
            
        if(GPad.getRawButton(5)){
            Solinoid();
        }else{ 
            SolinoidOff();
        }
        if(GPad.getRawButton(6)){
            Shooter();
        }
        else{
            ShooterOff();
        }
        if(GPad.getRawButton(4)){
            
                BridgeArm.set(1);

            
        }else if(GPad.getRawButton(2)){
            
                BridgeArm.set(-1);
            
        } else{
            
                BridgeArm.set(0);
            
        }
    }
    
    public double distance(double a, double d)  {
        double feet = 458.37*MathUtils.pow((a/d),(-.8464));
        return feet;
    }
    public void Solinoid() {
        Solonoid.set(Relay.Value.kOn);
    }
    public void SolinoidOff() {
        Solonoid.set(Relay.Value.kOff);
    }
    public void BallPickUp(){

            sweeper.set(1);
            conveyer.set(-1);

    }
       public void BallPickStop(){

            sweeper.set(0);
            conveyer.set(0);

    }
          public void BallPickDown(){

            sweeper.set(-1);
            conveyer.set(1);

    }
    

    public void Drive() {
        drive.tankDrive(GPadDrive.getY(), GPadDrive.getRawAxis(4));
    }
    
    
    public void Shooter(){
        try {
            shooterA.setX(1);
            shooterB.setX(1);
            shooterC.setX(1);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }

    }
    public void ShooterOff(){
        try {
            shooterA.setX(0);
            shooterB.setX(0);
            shooterC.setX(0);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }

    }

}
